/*
 * @Author: FamiliennameistChow nameistchow@hotmail.com
 * @Date: 2023-11-07 21:42:20
 * @LastEditors: FamiliennameistChow nameistchow@hotmail.com
 * @LastEditTime: 2023-11-07 22:08:01
 * @FilePath: /pcl_tutorials/pcl_trans_demo.cpp
 * @Description: 
 * 
 * Copyright (c) 2023 by FamiliennameistChow, All Rights Reserved. 
 */
#include <iostream>
#include <string>
#include <pcl/common/transforms.h> 
#include "PointCloudIO.hpp"
#include "PointCloudView.hpp"

int main(int argc, char **argv){

    std::cout << " This is a point transfrom demo " << std::endl;

    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);

    PointCloudIO::loadPointCloud("../data/bun000.ply", cloud);
    std::cout << "Loaded " << cloud->points.size() << " data points from test_pcd.pcd" << std::endl;

    pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZI>);

    //需要定义一个旋转矩阵
    // 举个例子，说明下转换矩阵是如何工作的。
    /* Reminder: how transformation matrices work :
    最右边一列的前3行是控制平移，所以x,y,z这三个值会平移数据。
    其中，x值控制沿着x轴上的平移
            |-------> This column is the translation。平移
    | 1 0 0 x |  \
    | 0 1 0 y |   }-> The identity 3x3 matrix (no rotation) on the left。旋转。全部是1，则没有变化
    | 0 0 1 z |  /
    | 0 0 0 1 |    -> We do not use this line (and it has to stay 0,0,0,1)。这一行不会改变数据。
    */
    
    /*-----------------------------------------------------------------
    METHOD #1: Using a Matrix4f
    This is the "manual" method, perfect to understand but error prone !
    这个是手动设置的4x4矩阵值，容易理解，但是也容易出错！
    */
    // 定义4x4转换矩阵
    // 前面3x3控制旋转，最右列控制平移
    Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();  // 4x4的单位矩阵
    // Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
    float theta = M_PI / 4; // The angle of rotation in radians（旋转角度，即弧度）
    transform_1(0, 0) = std::cos(theta);  // 设置这4个值，可以绕z轴旋转theta角度
    transform_1(0, 1) = -sin(theta);
    transform_1(1, 0) = sin(theta);
    transform_1(1, 1) = std::cos(theta);

    // Define a translation of 0.4 meters on the x axis.
    transform_1(0, 3) = 0.4;  // 沿着x平移0.4

    // Print the transformation
    std::cout<< "Method #1: using a Matrix4f" << std::endl;
    std::cout << transform_1 << std::endl;


    /*-----------------------------------------------------------------
    METHOD #2: Using a Affine3f
    因为是通过函数定义，所以此法更简单，且不容易出错
    */

    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();

    // Define a translation of 0.4 meters on the x axis.
    // 1，定义平移
    transform_2.translation() << 0.4, 0.0, 0.0;
    // The same rotation matrix as before; theta radians around Z axis
    // 2，定义然后Z旋转
    transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));  // UnitZ 即z轴



    pcl::transformPointCloud(*cloud, *transformed_cloud, transform_2);
    std::cout << "Transformed cloud by Eigen affine transformation" << std::endl;

    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> clouds;
    clouds.push_back(cloud);
    clouds.push_back(transformed_cloud);

    PointCloudView::cloudsVecView(clouds);

    //PointCloudIO::savePointCloud("../data/bun000_transformed.ply", transformed_cloud, false);
    //std::cout << "Saved " << transformed_cloud->points.size() << "data points to test_pcd_transformed." << std::endl;
}
